import math
from ctypes import *

import numpy as np
import numpy.ctypeslib as npct
from numpy.core.defchararray import index
from numpy.core.records import record

#label  index  black 1  green  2 white  3
file = open("/home/st/ubuntu_data/dataset/gazebo/city/0711/testing/trajectory/black.txt", "r")
tra_list = file.readlines()
black_gt = []
for i in range(len(tra_list)):
    black_gt.append(np.array(tra_list[i].strip().split(' '), dtype = np.float32 ))
black_gt = np.array(black_gt)
file.close()
file = open("/home/st/ubuntu_data/dataset/gazebo/city/0711/testing/trajectory/green.txt", "r")
tra_list = file.readlines()
green_gt = []
for i in range(len(tra_list)):
    green_gt.append(np.array(tra_list[i].strip().split(' '), dtype = np.float32 ))
green_gt = np.array(green_gt)
file.close()
file = open("/home/st/ubuntu_data/dataset/gazebo/city/0711/testing/trajectory/white.txt", "r")
tra_list = file.readlines()
white_gt = []
for i in range(len(tra_list)):
    white_gt.append(np.array(tra_list[i].strip().split(' '), dtype = np.float32 ))
white_gt = np.array(white_gt)
file.close()
file = open("/home/st/ubuntu_data/dataset/gazebo/city/0711/testing/trajectory/master.txt", "r")
tra_list = file.readlines()
master_gt = []
for i in range(len(tra_list)):
    master_gt.append(np.array(tra_list[i].strip().split(' '), dtype = np.float32 ))
master_gt = np.array(master_gt)
file.close()

#read data record
file = open("/home/st/ubuntu_data/exp_record/beijing/0727/1.txt", "r")
tra_list = file.readlines()
record_data = []
for i in range(len(tra_list)):
    record_data.append(np.array(tra_list[i].strip().split(' '), dtype = np.float32 ))
# file = open("/home/st/ubuntu_data/exp_record/beijing/0525b/need/226.txt", "r")
# tra_list = file.readlines()
# for i in range(len(tra_list)):
#     record_data.append(np.array(tra_list[i].strip().split(' '), dtype = np.float32 ))
# file = open("/home/st/ubuntu_data/exp_record/beijing/0525b/need/338.txt", "r")
# tra_list = file.readlines()
# for i in range(len(tra_list)):
#     record_data.append(np.array(tra_list[i].strip().split(' '), dtype = np.float32 ))
# file = open("/home/st/ubuntu_data/exp_record/beijing/0525b/need/441.txt", "r")
# tra_list = file.readlines()
# for i in range(len(tra_list)):
#     record_data.append(np.array(tra_list[i].strip().split(' '), dtype = np.float32 ))
# file = open("/home/st/ubuntu_data/exp_record/beijing/0525b/need/552.txt", "r")
# tra_list = file.readlines()
# for i in range(len(tra_list)):
#     record_data.append(np.array(tra_list[i].strip().split(' '), dtype = np.float32 ))
record_data = np.array(record_data)
file.close()

black_mea = []
green_mea = []
white_mea = []
for i in range(record_data.shape[0]):
    if record_data[i, 1] == 1:
        black_mea.append(record_data[i, :])
    if record_data[i, 1] == 2:
        green_mea.append(record_data[i, :])
    if record_data[i, 1] == 3:
        white_mea.append(record_data[i, :])
black_mea = np.array(black_mea)
green_mea = np.array(green_mea)
white_mea = np.array(white_mea)

#pos error
def pos_error_fun(black_mea, black_gt, master_gt):
    pos_error_radio_black = []
    for i in range(black_mea.shape[0]):
        index = np.where(black_gt[:, 0] > black_mea[i, 0] )
        index_front = index[0][0] - 1
        index_back = index[0][0]
        ratio = (black_mea[i, 0] - black_gt[index_front, 0])/(black_gt[index_back, 0]  -black_gt[index_front, 0] )
        pos_interval_x =  ratio*(black_gt[index_back, 1]  - black_gt[index_front, 1] ) + black_gt[index_front, 1]
        pos_interval_y =  ratio*(black_gt[index_back, 2]  - black_gt[index_front, 2] ) + black_gt[index_front, 2]
        pos_interval_z =  ratio*(black_gt[index_back, 3]  - black_gt[index_front, 3] ) + black_gt[index_front, 3]

        # error = (pos_interval_x - black_mea[i, 2])**2 + (pos_interval_y - black_mea[i, 3])**2  + (pos_interval_z - black_mea[i, 4] + 0.3)**2  #z pos bias
        error = (pos_interval_x - black_mea[i, 2])**2 + (pos_interval_y - black_mea[i, 3])**2
        error = math.sqrt(error) 

        index = np.where(master_gt[:, 0] > black_mea[i, 0] )
        index_front = index[0][0] - 1
        index_back = index[0][0]
        ratio = (black_mea[i, 0] - master_gt[index_front, 0])/(master_gt[index_back, 0]  -master_gt[index_front, 0] )
        master_pos_interval_x =  ratio*(master_gt[index_back, 1]  - master_gt[index_front, 1] ) + master_gt[index_front, 1]
        master_pos_interval_y =  ratio*(master_gt[index_back, 2]  - master_gt[index_front, 2] ) + master_gt[index_front, 2]
        master_pos_interval_z =  ratio*(master_gt[index_back, 3]  - master_gt[index_front, 3] ) + master_gt[index_front, 3]

        # distance_gt = (pos_interval_x - master_pos_interval_x)**2 + (pos_interval_y - master_pos_interval_y)**2  + (pos_interval_z - master_pos_interval_z + 0.1251)**2  #z pos bias
        distance_gt = (pos_interval_x - master_pos_interval_x)**2 + (pos_interval_y - master_pos_interval_y)**2
        distance_gt =math.sqrt(distance_gt) 
        pos_error_radio_black.append(float(error/distance_gt))
    return np.array(pos_error_radio_black)

#pos 
pos_error_radio_black = pos_error_fun(black_mea, black_gt, master_gt)
pos_error_radio_green = pos_error_fun(green_mea, green_gt, master_gt)
pos_error_radio_white = pos_error_fun(white_mea, white_gt, master_gt)
# print("pos_error_radio_green ", np.mean(pos_error_radio_green))
print("pos_error_radio_black ", np.mean(pos_error_radio_black), "pos_error_radio_green ", np.mean(pos_error_radio_green), "pos_error_radio_white ", np.mean(pos_error_radio_white))

#vel error
def vel_error_fun(black_mea, black_gt, master_gt):
    vel_error_radio_black = []
    for i in range(black_mea.shape[0]):
        index = np.where(black_gt[:, 0] > black_mea[i, 0] )
        index_front = index[0][0] - 1
        index_back = index[0][0]
        ratio = (black_mea[i, 0] - black_gt[index_front, 0])/(black_gt[index_back, 0]  -black_gt[index_front, 0] )
        vel_interval_x =  ratio*(black_gt[index_back, 8]  - black_gt[index_front, 8] ) + black_gt[index_front, 8]
        vel_interval_y =  ratio*(black_gt[index_back, 9]  - black_gt[index_front, 9] ) + black_gt[index_front, 9]
        vel_interval_z =  ratio*(black_gt[index_back, 10]  - black_gt[index_front, 10] ) + black_gt[index_front, 10]

        error = (vel_interval_x - black_mea[i, 5])**2 + (vel_interval_y - black_mea[i, 6])**2  + (vel_interval_z - black_mea[i, 7] )**2 
        error = math.sqrt(error) 

        # index = np.where(master_gt[:, 0] > black_mea[i, 0] )
        # index_front = index[0][0] - 1
        # index_back = index[0][0]
        # ratio = (black_mea[i, 0] - master_gt[index_front, 0])/(master_gt[index_back, 0]  -master_gt[index_front, 0] )
        # master_vel_interval_x =  ratio*(master_gt[index_back, 8]  - master_gt[index_front, 8] ) + master_gt[index_front, 8]
        # master_vel_interval_y =  ratio*(master_gt[index_back, 9]  - master_gt[index_front, 9] ) + master_gt[index_front, 9]
        # master_vel_interval_z =  ratio*(master_gt[index_back, 10]  - master_gt[index_front, 10] ) + master_gt[index_front, 10]
        #
        # vel_gt = (vel_interval_x - master_vel_interval_x)**2 + (vel_interval_y - master_vel_interval_y)**2  + (vel_interval_z - master_vel_interval_z)**2
        # vel_gt =math.sqrt(vel_gt)

        black_get_xyz = (vel_interval_x)**2 + (vel_interval_y )**2  + (vel_interval_z )**2
        black_get_xyz = math.sqrt(black_get_xyz)

    vel_error_radio_black.append(float(error/black_get_xyz))
    return np.array(vel_error_radio_black)

#vel error
vel_error_radio_black = vel_error_fun(black_mea, black_gt, master_gt)
vel_error_radio_green = vel_error_fun(green_mea, green_gt, master_gt)
vel_error_radio_white = vel_error_fun(white_mea, white_gt, master_gt)
# print("vel_error_radio_green ", np.mean(vel_error_radio_green))
print("vel_error_radio_black ", np.mean(vel_error_radio_black), "vel_error_radio_green ", np.mean(vel_error_radio_green), "vel_error_radio_white ", np.mean(vel_error_radio_white))

#pre pos error
def interval_gt(temp_t, temp_t_front, temp_t_back, temp_x_front, temp_x_back, temp_y_front, temp_y_back, temp_z_front, temp_z_back):
    ratio = (temp_t - temp_t_front)/(temp_t_back  - temp_t_front )
    vel_interval_x =  ratio*(temp_x_back  - temp_x_front ) + temp_x_front
    vel_interval_y =  ratio*(temp_y_back  - temp_y_front) + temp_y_front
    vel_interval_z =  ratio*(temp_z_back  - temp_z_front) + temp_z_front
    return vel_interval_x, vel_interval_y, vel_interval_z

def pre_error_fun(black_mea, black_gt, master_gt):
    pos_error_radio_all = []
    for i in range(black_mea.shape[0]):
        time_stamp_base = black_mea[i, 0]

        index = np.where(master_gt[:, 0] > time_stamp_base)
        index_front = index[0][0] - 1
        index_back = index[0][0]
        master_pos_interval_gt_x, master_pos_interval_gt_y, master_pos_interval_gt_z = interval_gt(time_stamp_base, master_gt[index_front, 0], master_gt[index_back, 0],\
                master_gt[index_front, 1], master_gt[index_back, 1],\
                master_gt[index_front, 2], master_gt[index_back, 2],\
                master_gt[index_front, 3], master_gt[index_back, 3])

        pos_error_radio = []
        for j in range(10):
            index = np.where(black_gt[:, 0] > time_stamp_base + j*1/9 )
            index_front = index[0][0] - 1
            index_back = index[0][0]
            pre_pos_interval_gt_x, pre_pos_interval_gt_y, pre_pos_interval_gt_z = interval_gt(time_stamp_base + j*1/9, black_gt[index_front, 0], black_gt[index_back, 0],\
                black_gt[index_front, 1], black_gt[index_back, 1], \
                black_gt[index_front, 2], black_gt[index_back, 2],\
                black_gt[index_front, 3], black_gt[index_back, 3])

            error = (pre_pos_interval_gt_x - black_mea[i, 8 + j*3])**2 + (pre_pos_interval_gt_y - black_mea[i, 9 + j*3])**2  + (pre_pos_interval_gt_z - black_mea[i, 10 + j*3] + 0.3 )**2  #z pos bias
            error = math.sqrt(error) 

            distance_gt = (pre_pos_interval_gt_x - master_pos_interval_gt_x)**2 + (pre_pos_interval_gt_y - master_pos_interval_gt_y)**2  + (pre_pos_interval_gt_z - master_pos_interval_gt_z + 0.1251)**2  #z pos bias
            distance_gt =math.sqrt(distance_gt) 
            pos_error_radio.append(float(error/distance_gt))

        pos_error_radio = np.array(pos_error_radio)
        pos_error_radio_all.append(np.mean(pos_error_radio))
        
    return np.array(pos_error_radio_all)

#pre pos error
pre_error_radio_black = pre_error_fun(black_mea, black_gt, master_gt)
pre_error_radio_green = pre_error_fun(green_mea, green_gt, master_gt)
pre_error_radio_white = pre_error_fun(white_mea, white_gt, master_gt)
# print("pre_error_radio_green ", np.mean(pre_error_radio_green))
print("pre_error_radio_black ", np.mean(pre_error_radio_black), "pre_error_radio_green ", np.mean(pre_error_radio_green), "pre_error_radio_white ", np.mean(pre_error_radio_white))

